/*
 * YawCircle.cpp
 *
 *  Created on: 2023年5月30日
 *      Author: Jiapeng Chen
 **   带偏航角的定高圆周运动
 */
#ifndef MODULES_YAWCIRCLE_HPP_
#define MODULES_YAWCIRCLE_HPP_

#include "system/system.hpp"
#include "userlib/userlib.hpp"

#ifdef __cplusplus
extern "C" {
#endif


#ifdef __cplusplus
}
#endif

#ifdef __cplusplus
#include <Eigen>
using namespace Eigen;
using namespace std;

class YAW_CIRCLE
{
public:
//	TRAJECTORY_LISSAJOUS(){}
	YAW_CIRCLE(float d0 = 0.01f, float A0 = 0.8f, float B0 = 0.8f)
	{
		this -> d = d0;
		this -> A = A0;
		this -> B = B0;
	}
	~YAW_CIRCLE(){}
	float getPos_x(float t);
	float getPos_y(float t);
	float getPos_z(float t);
	float getYaw(float t);
	float getOffset_x();
	float getOffset_y();
	float getOffset_z();
	void setOffset(float val_x,float val_y,float val_z);
private:
	float d;
	float offset_x;
	float offset_y;
	float offset_z;
	float A;
	float B;
	float C;
	float a;
	float b;
	float c;

	float allocationx;
	float allocationy;

	Matrix<float,3,3> R;
	Vector2f output;
	Vector2f control_output;
};

#endif


#endif
